/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-30     MBW       the first version
 */
#include "sys.h"
#include "JY901B_931.h"
#include "maths.h"
#include "filter.h"
#include "string.h"
#include "stdlib.h"
#include "stdio.h"
#include "niming_flow.h"
#include "data_define.h"
#include "sins.h"
#include "us016.h"
#include "delay_uil.h"
biquadFilter_t sins_Aac_FilterLPF[3];//陀螺仪数据二阶低通滤波器
void sensor_pro(void *p)
{
    float ax,ay,az;
    u32 cnt=0;
    float aac_z=0 ,aac_x =0 ,aac_y =0 ,fil_acc[3];
    F_XYZ vel,dis,dis_res;
    u32 t_now,t_preiod,t_pre ;
    void *delay;
    delay = delay_until_create("delay");  // 创建相对延时

    for (int axis = 0; axis < 3; axis++)//con_fre Hz
    {
        biquadFilterInitLPF(&sins_Aac_FilterLPF[axis], 200, 2);
    }
    rt_thread_mdelay(1000);
    dis_res.x = ano_of.intergral_x , dis_res.y = -ano_of.intergral_y;
    while(1)
    {
//        control_data.batter_vol = Get_batter_Adc_Average(0,10);
        //**********获取传感器姿态数据**********
        get_JY901B();//三轴加速度，陀螺仪数据已低通滤波
        ax = JY901_data.aac.x;      ay = JY901_data.aac.y;      az = JY901_data.aac.z; //单位一个重力加速度g
        //**********求旋转矩阵,得到导航系下的三轴速度aac_x aac_y aac_z**********
        aac_x =    Mat.DCM[0][0] * ax   + Mat.DCM[0][1] * ay  + Mat.DCM[0][2] * az ;//单位一个重力加速度g
        aac_y =    Mat.DCM[1][0] * ax   + Mat.DCM[1][1] * ay  + Mat.DCM[1][2] * az ;//单位一个重力加速度g
        aac_z =    Mat.DCM[2][0] * ax   + Mat.DCM[2][1] * ay  + Mat.DCM[2][2] * az -1.0016;//单位一个重力加速度g
        aac_z = aac_z*980;
        fil_acc[0] = aac_x ,fil_acc[1] = aac_y ,fil_acc[2] = aac_z;
        for (int axis = 0; axis < 3; axis++)
        {
            fil_acc[axis]   = biquadFilterApply(&sins_Aac_FilterLPF[axis], fil_acc[axis]);
        }
        //**********获取传感器姿态数据**********
        dis.x = ano_of.intergral_x , dis.y = -ano_of.intergral_y;

        if(ano_of.of_alt_cm<250)  dis.z = ano_of.of_alt_cm;
//        dis.z =  JY901_data.height;
        vel.x = ano_of.of2_dx_fix , vel.y = -ano_of.of2_dy_fix ;
        sins_high(fil_acc[2],dis.z);

        vel.z = sins.vel.z;
//        if(cnt>2000)control_data.velz2 = control_data.velz2+vel.z ;

        control_data.get_aac.z = fil_acc[2];

        control_data.get_gyro.x = JY901_data.gyro.x;
        control_data.get_gyro.y = JY901_data.gyro.y;
        control_data.get_gyro.z = JY901_data.gyro.z;

        control_data.get_angel.x = JY901_data.angel.x;
        control_data.get_angel.y = JY901_data.angel.y;
        control_data.get_angel.z = JY901_data.angel.z;

        control_data.get_position.x = dis.x-dis_res.x;
        control_data.get_position.y = dis.y-dis_res.y;
        control_data.get_position.z = dis.z;

        control_data.get_vel.x = vel.x;
        control_data.get_vel.y = vel.y;
        control_data.get_vel.z = vel.z;
        //**********任务运行时间统计**********
        t_now = rt_tick_get();//要赋值给整型数据
        t_preiod=t_now-t_pre;
        task_time1.senor_time = 1000/t_preiod;
        t_pre = t_now;
//        rt_thread_mdelay(5);
        cnt++;
        delay_until_run(delay, 5);
    }
}
void sensor_pro_init(void)
{
    rt_thread_t tid1 = RT_NULL;
    tid1 = rt_thread_create("sensor_pro",sensor_pro, RT_NULL, 2000, sen_task_prio, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...sensor_pro is ok!\n");
}
INIT_APP_EXPORT(sensor_pro_init);
